#include "physics/physics_object.hpp"

#include <string>
#include <vector>

#include "irrlicht.h"
using namespace irr;

#include "graphics/irr_driver.hpp"
#include "graphics/mesh_tools.hpp"
#include "io/file_manager.hpp"
#include "io/xml_node.hpp"
#include "modes/world.hpp"
#include "tracks/track.hpp"
#include "utils/constants.hpp"
#include "utils/coord.hpp"
#include "utils/string_utils.hpp"

PhysicalObject::PhysicalObject(const XMLNode& xml_node)
	: TrackObject(xml_node)
{
	m_shape = NULL;
	m_body = NULL;
	m_motion_state = NULL;
	m_mass = 1;
	m_radius = -1;
	
	std::string shape;
	xml_node.get("mass", &m_mass);
	xml_node.get("radius", &m_radius);
	xml_node.get("shape", &shape);
	
	m_body_type = MP_NONE;
	if (shape == "cone")
		m_body_type = MP_CONE;
	else if (shape == "box")
		m_body_type = MP_BOX;
	else if (shpe == "sphere")
		m_body_type = MP_SPHERE;
		
	m_init_pos.setIdentity();
	m_init_pos.setOrigin(m_init_xyz);
}

PhysicalObject::~PhysicalObject()
{
	World::getWorld()->getPhysics()->removeBody(m_body);
	delete m_body;
	delete m_motion_state;
	delete m_shape;
}

void PhysicalObject::init()
{
	assert(m_animated_mesh);
	
	Vec3 min, max;
	MeshTools::minMax3D(m_animated_mesh, &min, &max);
	Vec3 extend = max - min;
	Vec3 offset_from_center = -0.5f * (max + min);
	switch(m_body_type)
	{
		case MP_CONE:
		{
			if (m_radius < 0)
				m_radius = 0.5f * extend.length_2d();
				m_shape = new btConeShapeZ(m_radius, extend.getZ());
				break;
		}
		case MP_BOX: m_shape = new btBoxShape(0.5 * extend);
					 break;
		case MP_SPHERE:
		{
			if (m_radius < 0)
			{
				m_radius = std::max(extend.getX(), extend.getY());
				m_radius = 0.5f * std::max(m_radius, extend.getZ());
			}
			m_shape = new btSphereShape(m_radius);
			break;
		}
		case MP_NONE:
			fprintf(stderr, "WARNING: Uninitialised moving shape\n");
			break;
	}
	
	scene::IMeshManipulator *mesh_manipulator = 
		irr_driver->getSceneManager()->getMeshManipulator();
	core::matrix4 transform(core::matrix4::EM4CONST_IDENTITY);
	transform.setTranslation(offset_from_center.toIrrVector());
	mesh_manipulator->transformMesh(m_animated_mesh, transform);
	
	m_init_pos.setOrigin(m_init_pos.getOrigin() + btVector3(0, 0, extend.getZ() * 0.5f));
	m_motion_state = new btDefaultMotionState(m_init_pos);
	btVector3 inertia;
	m_shape->calculateLocalInertia(m_mass, inertia);
	btRigidBody::btRigidBodyConstructionInfo info(m_mass, m_motion_state, m_shape, inertia);
	
	info.m_angularDamping = 0.5f;
	m_body = new btRigidBody(info);
	m_user_pointer.set(this);
	m_body->setUserPointer(&m_user_pointer);
	
	World::getWorld()->getPhysics()->addBody(m_body);
}

void PhysicalObject::update(float dt)
{
	btTransform t;
	m_motion_state->getWorldTransform(t);
	
	Coord c(t);
	if (c.getXYZ().getZ() < -100)
	{
		m_body->setCenterOfMassTransform(m_init_pos);
		c.setXYZ(m_init_pos.getOrigin());
	}
	m_animated_node->setPosition(c.getXYZ().toIrrVector());
	btQuaternion q = t.getRotation();
	core::quaternion qirr(q.getX(), q.getZ(), q.getY(), -q.getW());
	core::vector3df r;
	qirr.toEuler(r);
	m_animated_node->setRotation(r * RAD_TO_DEGREE);
	return;
}

void PhysicalObject::reset()
{
	m_body->setCenterOfMassTransform(m_init_pos);
	m_body->setAngularVelocity(btVector3(0, 0, 0));
	m_body->setLinearVelocity(btVector3(0, 0, 0));
	m_body->activate();
}

void PhysicalObject::handleExplosion(const Vec3& pos, bool direct_hit)
{
	if (direct_hit)
	{
		btVector3 impulse(0.0f, 0.0f, stk_config->m_explosion_impulse_objects);
		m_body->applyCentralImpulse(impulse);
	}
	else
	{
		btTransform t;
		m_motion_state->getWorldTransform(t);
		btVector3 diff = t.getOrigin() - pos;
		
		float len2 = diff.length2();
		
		btVector3 impulse = diff * stk_config->m_explosion_impulse_objects / lens;
		m_body->applyCentralImpulse(impulse);
	}
	
	m_body->activate();
}
